/******************************************************************************/
/* modbus.c                                                                   */
/*                                                                            */
/* Based on "libmodbus-2.0.3" - Unix\Win version                              */
/* Author: Stephane Raimbault <stephane.raimbault@gmail.com>                  */
/*                                                                            */
/* Modifed for embedded system 02.2011 Sergei Maslyakov <rusoil.9@gmail.com>  */
/******************************************************************************/


#include "data_types.h"
#include "crc16.h"
#include "modbus.h"


/******************************************************************************/
/* Example                                                                    */
/******************************************************************************/
/*
   1. OpenMbMaster() - init uart, set slave addr and func
   2. Modbus_make_query() - create modbus query. Input structure see in
      modbus_def.h.
      Func return error(<0) or expected answer datalen.
      Expected answer datalen need for receive and parsing answer.

   3. Send query
   4. Wait answer and parsing
      If you used rtos, send and receive task desirable must have separate.
      Received data bytes need send in function Modbus_receive() and test
      return value.
   5. After receiving data - need run func Modbus_process_answ() - its func
      validate and select payload data from answer. Need test return value for
      error.
   6. CloseMbMaster() - need if you used driver rs485 or for free alloc memory
*/





/******************************************************************************/
/*      Static variables                                                      */
/******************************************************************************/
static __MB_ANSW_READY_DATA * s_pAnsw;
static __MB_QUERY_TOOLS       s_query;






//==============================================================================
// App: Modbus_change_order_bytes()
//==============================================================================
static void Modbus_change_order_bytes(uint8_t * const _pBuff,
                                      const uint32_t _dataLen) {

    uint8_t swop;

    for (uint32_t i = 0; i < _dataLen; i++) {
        swop = _pBuff[i + 1];
        _pBuff[i + 1] = _pBuff[i];
        _pBuff[i] = swop;
        i++;
    };
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Compute_response_length()
//==============================================================================
static uint32_t Compute_response_length(const uint8_t * const _pQuery) {

    uint32_t length;

    switch (_pQuery[1])
    {
        //----------------------------------------------------------------------
        case FC_READ_COIL_STATUS:
        //----------------------------------------------------------------------
        case FC_READ_INPUT_STATUS:
          {
              // Header + nb values (code from force_multiple_coils)
              uint32_t nb = (_pQuery[4] << 8) | _pQuery[5];
              length = 3 + (nb / 8) + ((nb % 8) ? 1 : 0);
          };
        break;

        //----------------------------------------------------------------------
        case FC_READ_HOLDING_REGISTERS:
        //----------------------------------------------------------------------
        case FC_READ_INPUT_REGISTERS:
          // Header + 2 * nb values
          length = 3 + 2 * ((_pQuery[4] << 8) | _pQuery[5]);
        break;

        //----------------------------------------------------------------------
        case FC_FORCE_SINGLE_COIL:
        //----------------------------------------------------------------------
        case FC_PRESET_SINGLE_REGISTER:
        //----------------------------------------------------------------------
        case FC_FORCE_MULTIPLE_COILS:
        //----------------------------------------------------------------------
        case FC_PRESET_MULTIPLE_REGISTERS:
          length = 6;
        break;

        //----------------------------------------------------------------------
        default:
          return MSG_LENGTH_UNDEFINED;
    };

    return (length + CHECKSUM_LENGTH_RTU);
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Build_query_basis_rtu()
//==============================================================================
static uint32_t Build_query_basis_rtu(const uint8_t _slave,
                                      const uint8_t _function,
                                      const uint16_t _startAddr,
                                      const uint16_t _nb,
                                      uint8_t * const _pQuery) {

    _pQuery[0] = _slave;
    _pQuery[1] = _function;
    _pQuery[2] = _startAddr >> 8;
    _pQuery[3] = _startAddr & 0x00ff;
    _pQuery[4] = _nb >> 8;
    _pQuery[5] = _nb & 0x00ff;

    return PRESET_QUERY_LENGTH_RTU;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Check_crc16()
//==============================================================================
static sint32_t Check_crc16(const uint8_t * const _pMsg,
                            const uint32_t _msgLength) {

    const uint16_t crc_calc = ModbusCrc16(_pMsg, _msgLength - 2);
    const uint16_t crc_received = (_pMsg[_msgLength - 1] << 8)
                                      | _pMsg[_msgLength - 2];

    // Check CRC
    return (crc_calc == crc_received) ? MB_MASTER_ERR_OK : MB_MASTER_ERR_CRC;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Modbus_add_crc()
//==============================================================================
static uint32_t Modbus_add_crc(uint8_t * const _pQuery,
                               const uint32_t _queryLength) {

    const uint16_t crc = ModbusCrc16(_pQuery, _queryLength);
    _pQuery[_queryLength] = (uint8_t)(crc & 0x00FF);
    _pQuery[_queryLength + 1] = (uint8_t)(crc >> 8);
    return 2;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Read_io_status()
//==============================================================================
static void Read_io_status(const uint8_t _slave,
                           const uint8_t _function,
                           const uint16_t _startAddr,
                           const uint16_t _nb) {

    s_query.queryLen = Build_query_basis_rtu(_slave,
                                             _function, 
                                             _startAddr,
                                             _nb,
                                             s_query.pQuery);

    s_query.queryLen += Modbus_add_crc(s_query.pQuery, s_query.queryLen);
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Read_registers()
//==============================================================================
static sint32_t Read_registers(const uint8_t _slave,
                               const uint8_t _function,
                               const uint16_t _startAddr,
                               const uint16_t _nb) {
    if (_nb > MAX_REGISTERS) {
        return MB_MASTER_ERR_LEN;
    };

    s_query.queryLen = Build_query_basis_rtu(_slave,
                                             _function,
                                             _startAddr,
                                             _nb,
                                             s_query.pQuery);

    s_query.queryLen += Modbus_add_crc(s_query.pQuery, s_query.queryLen);

    return MB_MASTER_ERR_OK;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Set_single()
//==============================================================================
static void Set_single(const uint8_t _slave,
                       const uint8_t _function,
                       const uint16_t _addr,
                       const uint16_t _value) {

    s_query.queryLen = Build_query_basis_rtu(_slave,
                                             _function, 
                                             _addr,
                                             _value,
                                             s_query.pQuery);

    s_query.queryLen += Modbus_add_crc(s_query.pQuery, s_query.queryLen);
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Read_coil_status()
//==============================================================================
static sint32_t Read_coil_status(const uint8_t _slave,
                                 const uint16_t _startAddr,
                                 const uint16_t _nb) {
    if (_nb > MAX_STATUS) {
        return MB_MASTER_ERR_LEN;
    };

    Read_io_status(_slave, FC_READ_COIL_STATUS, _startAddr, _nb);

    return MB_MASTER_ERR_OK;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Read_input_status()
//==============================================================================
static sint32_t Read_input_status(const uint8_t _slave,
                                  const uint16_t _startAddr,
                                  const uint16_t _nb) {
    if (_nb > MAX_STATUS) {
        return MB_MASTER_ERR_LEN;
    };

    Read_io_status(_slave, FC_READ_INPUT_STATUS, _startAddr, _nb);

    return MB_MASTER_ERR_OK;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Read_holding_registers()
//==============================================================================
static sint32_t Read_holding_registers(const uint8_t _slave,
                                       const uint16_t _startAddr,
                                       const uint16_t _nb) {
    if (_nb > MAX_REGISTERS) {
        return MB_MASTER_ERR_LEN;
    };

    return Read_registers(_slave,
                          FC_READ_HOLDING_REGISTERS,
                          _startAddr,
                          _nb);
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Read_input_registers()
//==============================================================================
static sint32_t Read_input_registers(const uint8_t _slave,
                                     const uint16_t _startAddr,
                                     const uint16_t _nb) {
    if (_nb > MAX_REGISTERS) {
        return MB_MASTER_ERR_LEN;
    };

    return Read_registers(_slave,
                          FC_READ_INPUT_REGISTERS,
                          _startAddr,
                          _nb);
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Force_single_coil()
//==============================================================================
static void Force_single_coil(const uint8_t _slave,
                              const uint16_t _coilAddr,
                              uint32_t _state) {
    if (_state) {
        _state = 0xFF00;
    };

    Set_single(_slave, FC_FORCE_SINGLE_COIL, _coilAddr, _state);
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Preset_single_register()
//==============================================================================
static void Preset_single_register(const uint8_t _slave,
                                   const uint16_t _regAddr,
                                   const uint16_t _value) {

    Set_single(_slave, FC_PRESET_SINGLE_REGISTER, _regAddr, _value);
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Force_multiple_coils()
//==============================================================================
static sint32_t Force_multiple_coils(const uint8_t _slave,
                                     const uint16_t _startAddr,
                                     const uint16_t _nb,
                                     const uint8_t * const _pData) {
    if (_nb > MAX_STATUS) {
        return MB_MASTER_ERR_LEN;
    };

    s_query.queryLen = Build_query_basis_rtu(_slave,
                                             FC_FORCE_MULTIPLE_COILS, 
                                             _startAddr,
                                             _nb,
                                             s_query.pQuery);

    const uint32_t byteCount = (_nb / 8) + ((_nb % 8) ? 1 : 0);
    s_query.pQuery[s_query.queryLen++] = byteCount;

    uint32_t coilCheck = 0;
    uint32_t pos = 0;

    for (uint32_t i = 0; i < byteCount; i++) {
        int32_t bit;

        bit = 0x01;
        s_query.pQuery[s_query.queryLen] = 0;

        while ((bit & 0xFF) && (coilCheck++ < _nb)) {

            if (_pData[pos++]) {
                s_query.pQuery[s_query.queryLen] |= bit;
            }
            else {
                s_query.pQuery[s_query.queryLen] &=~ bit;
            };
            bit = bit << 1;
        };
        s_query.queryLen++;
    };

    s_query.queryLen += Modbus_add_crc(s_query.pQuery, s_query.queryLen);

    return MB_MASTER_ERR_OK;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Preset_multiple_registers()
//==============================================================================
static sint32_t Preset_multiple_registers(const uint8_t _slave,
                                          const uint16_t _startAddr,
                                          const uint16_t _nb,
                                          const uint8_t * const _pData) {
    if (_nb > MAX_REGISTERS) {
        return MB_MASTER_ERR_LEN;
    };

    s_query.queryLen = Build_query_basis_rtu(_slave,
                                             FC_PRESET_MULTIPLE_REGISTERS, 
                                             _startAddr,
                                             _nb,
                                             s_query.pQuery);

    s_query.pQuery[s_query.queryLen++] = _nb * 2;

    for (uint32_t i = 0; i < ((uint32_t)_nb); i++) {
        s_query.pQuery[s_query.queryLen++] = _pData[i];
        s_query.pQuery[s_query.queryLen++] = _pData[i + 1];
    };

    s_query.queryLen += Modbus_add_crc(s_query.pQuery, s_query.queryLen);

    return MB_MASTER_ERR_OK;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: User_func()
//==============================================================================
static sint32_t User_func(const uint8_t _slave,
                          const uint8_t _function,
                          const uint16_t _dataLen,
                          const uint8_t * const _pData) {
    // Test
    if (_dataLen > (MAX_REGISTERS * 2)) {
        return MB_MASTER_ERR_LEN;
    };

    if (_function > 127) {
        return MB_MASTER_ERR_FUNC;
    };

    // Make query
    s_query.queryLen = 0;

    s_query.pQuery[s_query.queryLen++] = _slave;
    s_query.pQuery[s_query.queryLen++] = _function;

    for (uint16_t i = 0; i < _dataLen; i++) {
        s_query.pQuery[s_query.queryLen++] = _pData[i];
    };

    // Add crc
    s_query.queryLen += Modbus_add_crc(s_query.pQuery, s_query.queryLen);

    return MB_MASTER_ERR_OK;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Open mb master
//==============================================================================
sint32_t OpenMbMaster(const uint32_t _baudrate,
                      const uint8_t _key,
                      const uint8_t _slave,
                      const uint8_t _func) {
    // Test
    if ((_baudrate == 0) || (_baudrate > 115200)
          || (_slave > 247) || (_func > 127)) {
        return (-1);
    };

    // Set param
    s_query.slave = _slave;
    s_query.func = _func;

    /* Need init uart, key = uart settings (example - 8N1) */
    //Init_Uart(_baudrate, _key);
    //DriverTxOn();

    return 0;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Close mb master
//==============================================================================
void CloseMbMaster() {

    /* Close uart, disable driver rs485
    DisableInt();
    DriverShutdown();
    */
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Modbus_make_query()
//
// Inputs (structure):
// - addr       (uint16_t)
// - value      (uint16_t)
// - data array (def size in modbus_def.h)
// If used non-standart function value = data len
//
// Outputs:
// < 0 - error
// > 0 - expected response lenght
// = 0 - response lenght is undefined (used non-standart function)
//==============================================================================
sint32_t Modbus_make_query(const __MB_QUERY_BUILD * const _pQueryData) {
    sint32_t err = 0;

    switch (s_query.func)
    {
        //----------------------------------------------------------------------
        case FC_READ_COIL_STATUS:
          err = Read_coil_status(s_query.slave,
                                 _pQueryData->addr,
                                 _pQueryData->value);
        break;

        //----------------------------------------------------------------------
        case FC_READ_INPUT_STATUS:
          err = Read_input_status(s_query.slave,
                                  _pQueryData->addr,
                                  _pQueryData->value);
        break;

        //----------------------------------------------------------------------
        case FC_READ_HOLDING_REGISTERS:
          err = Read_holding_registers(s_query.slave,
                                       _pQueryData->addr,
                                       _pQueryData->value);
        break;

        //----------------------------------------------------------------------
        case FC_READ_INPUT_REGISTERS:
          err = Read_input_registers(s_query.slave,
                                     _pQueryData->addr,
                                     _pQueryData->value);
        break;

        //----------------------------------------------------------------------
        case FC_FORCE_SINGLE_COIL:
          Force_single_coil(s_query.slave,
                            _pQueryData->addr,
                            _pQueryData->value);
        break;

        //----------------------------------------------------------------------
        case FC_PRESET_SINGLE_REGISTER:
          Preset_single_register(s_query.slave,
                                 _pQueryData->addr,
                                 _pQueryData->value);
        break;

        //----------------------------------------------------------------------
        case FC_FORCE_MULTIPLE_COILS:
          err = Force_multiple_coils(s_query.slave,
                                     _pQueryData->addr,
                                     _pQueryData->value,
                                     _pQueryData->pData);
        break;

        //----------------------------------------------------------------------
        case FC_PRESET_MULTIPLE_REGISTERS:
          err = Preset_multiple_registers(s_query.slave,
                                          _pQueryData->addr,
                                          _pQueryData->value,
                                          _pQueryData->pData);
        break;

        //----------------------------------------------------------------------
        default:
          err = User_func(s_query.func,
                          s_query.slave,
                          _pQueryData->value,
                          _pQueryData->pData);
        break;
    };

    if (err < 0) {
        return err;
    };

    s_query.expectedLen = Compute_response_length(s_query.pQuery);
    return s_query.expectedLen;
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Modbus_receive()
//
// Outputs:
// < 0 - error
// > 0 - return answLen - wait next data byte
// = 0 - answer received, crc ok or timeout expired (non-standart func)
//==============================================================================
sint32_t Modbus_receive(const uint8_t _byte) {
    static uint32_t state;

    // Test
    if ((s_pAnsw->answLen > MAX_MESSAGE_LENGTH)
        || (s_query.expectedLen > MAX_MESSAGE_LENGTH)) {
        return MB_MASTER_ERR_LEN;
    };

    // Init start parsing
    if (s_pAnsw->answLen == 0) {
        state = eMB_PARS_SLAVE_ADDR;
    };

    s_pAnsw->pAnsw[s_pAnsw->answLen++] = _byte;

    switch (state)
    {
        //----------------------------------------------------------------------
        case eMB_PARS_SLAVE_ADDR:
          state++;
          if (s_pAnsw->pAnsw[0] != s_query.slave) {
              return MB_MASTER_ERR_SLAVE;
          }
          else {
              return s_pAnsw->answLen;
          };

        //----------------------------------------------------------------------
        case eMB_PARS_FUNC:
          state++;
          if (s_pAnsw->pAnsw[1] != s_query.func) {
              s_query.expectedLen = 5;

              if (s_pAnsw->pAnsw[1] != (s_query.func | 0x80)) {
                  return MB_MASTER_ERR_FUNC;
              };
          };
          return s_pAnsw->answLen;

        //----------------------------------------------------------------------
        case eMB_PARS_DATA:
          if (s_query.expectedLen != s_pAnsw->answLen) {
              return s_pAnsw->answLen;
          };
        break;

        //----------------------------------------------------------------------
        default:
          return MB_MASTER_ERR_UNDEF;
    };

    return Check_crc16(s_pAnsw->pAnsw, s_pAnsw->answLen);
}
//==============================================================================
//==============================================================================


//==============================================================================
// App: Modbus_process_answ()
//
// Return:
// - < 0 - error
// = 0 - no error
//==============================================================================
sint32_t Modbus_process_answ() {
    sint32_t err;

    // Test at timeout
    if ((s_pAnsw->answLen == 0) || (s_pAnsw->answLen < MIN_ANSWER_LENGTH)) {
        return MB_MASTER_ERR_TIMEOUT;
    };

    // Test at user func (need test crc or not)
    if (s_pAnsw->answLen != s_query.expectedLen) {
        err = Check_crc16(s_pAnsw->pAnsw, s_pAnsw->answLen);

        if (err != MB_MASTER_ERR_OK) {
            return err;
        };
    };

    // Test on modbus exception
    if (s_query.func == (s_pAnsw->pAnsw[1] | 0x80)) {
        s_pAnsw->errCode = s_pAnsw->pAnsw[2];

        return MB_MASTER_ERR_OK;
    };

    // Calc payload data
    s_pAnsw->errCode = 0;

    switch (s_query.func)
    {
        //----------------------------------------------------------------------
        case FC_READ_COIL_STATUS:
        //----------------------------------------------------------------------
        case FC_READ_INPUT_STATUS:
          if ((s_pAnsw->answLen - 5) != ((uint32_t)s_pAnsw->pAnsw[2])) {
              return MB_MASTER_ERR_DATA;
          };

          s_pAnsw->pAnsw += 3;
          s_pAnsw->answLen -= 5;
        break;

        //----------------------------------------------------------------------
        case FC_READ_HOLDING_REGISTERS:
        //----------------------------------------------------------------------
        case FC_READ_INPUT_REGISTERS:
          if ((s_pAnsw->answLen - 5) != ((uint32_t)s_pAnsw->pAnsw[2])) {
              return MB_MASTER_ERR_DATA;
          };

          s_pAnsw->pAnsw += 3;
          s_pAnsw->answLen -= 5;
          Modbus_change_order_bytes(s_pAnsw->pAnsw, s_pAnsw->answLen);
        break;

        //----------------------------------------------------------------------
        case FC_FORCE_SINGLE_COIL:
        //----------------------------------------------------------------------
        case FC_FORCE_MULTIPLE_COILS:
        //----------------------------------------------------------------------
        case FC_PRESET_SINGLE_REGISTER:
        //----------------------------------------------------------------------
        case FC_PRESET_MULTIPLE_REGISTERS:
        break;

        //----------------------------------------------------------------------
        default:
          s_pAnsw->pAnsw += 2;
          s_pAnsw->answLen -= 4;
        break;
    };

    return MB_MASTER_ERR_OK;
}
//==============================================================================
//==============================================================================